function [sigmaPoints meanWeights covarWeights] = generateSigmaPoints(mu, covar, a, b, k)

if nargin < 3
    % a = 1;
    a = 1e-3;
    b = 2;
    k = 0;
end

% compute scaling factors
L = length(mu);
lambda = a*a*(L+k) - L;

% compute sigma points
sigmaPoints = sigmas(mu, covar, sqrt(L+lambda));

% compute mean and covar weights
meanWeights  = [0.5*ones(1, 2*L)/(L+lambda) lambda/(L+lambda)];
covarWeights = [0.5*ones(1, 2*L)/(L+lambda) lambda/(L+lambda)+(1-a*a+b)];



function X = sigmas(x, P, c)
%Sigma points around reference point
%Inputs:
%       x: reference point
%       P: covariance
%       c: coefficient
%Output:
%       X: Sigma points
%
% By Yi Cao at Cranfield University, 04/01/2008
%

% use sqrtm or cholesky
% A = c*sqrtm(P);
A = c*chol(P);

Y = x(ones(numel(x), 1), :);
X = [Y+A; Y-A; x];
